#include <Wire.h>
#include <Servo.h>
#include <EEPROM.h>
#include <Depecabot3.h>

Depecabot robot;

#define SERVO_INIT_POS 90

unsigned int modo=0;
unsigned int sub_modo=0;

unsigned long timer;

int distancia = 0;
int servo = 0;
int encoder1=0;
int encoder2=0;
int motor=0;
int tono=0;
float bateria= 0;
unsigned int posicion= 0;

unsigned char caracter=0;
unsigned char caracter_bueno=0;

void setup(void)
{
  robot.init ();
  robot.sensorLinea.init();

  robot.servo1.attach(SERVO);            //Inicializacion servo
  robot.servo1.write(SERVO_INIT_POS);
}
void loop(void)
{
  if(millis()-timer>200)
  {
    timer=millis();
    if (robot.pulsador(1))                //El pulsador 1 cambia de menu
    {
      modo++;
      sub_modo=0;
      motor=0;
      robot.motores(motor,motor);
      robot.lcd.clear();
    }
    switch(modo)
    {
    case 0:
      {
        break;
      }
    case 1: //para comprobar los GP2 en distancia
      {
        robot.lcd.setCursor(0,0);
        robot.lcd.print("SENSOR DIST.");	

        distancia=robot.gp2ymm(GP2);        //Lectura del GP2 en milimetros
        robot.lcd.setCursor(0,2);
        robot.lcd.print("Distancia=");
        robot.lcd.setCursor(54,3);
        robot.lcd.print(distancia);
        robot.lcd.print("mm  ");

        break;
      }
    case 2://comprobar los motores
      {
        if (sub_modo == 0)
        {
          robot.lcd.setCursor(0,0);
          robot.lcd.print("MOTORES");
          sub_modo = 1;
        }
        else if (sub_modo == 1)
        {
          if (robot.pulsador(3))
          {
            motor += 25;
            if (motor>255)                //El pulsador 3 incrementa la velocidad de los motores
              motor=0;
            robot.motores(motor,motor);
            robot.lcd.setCursor(0,2);
            robot.lcd.print("Velocidad=");
            robot.lcd.print(motor);
            robot.lcd.print("  ");
          }
          if (robot.pulsador(2))
          {
            motor -= 25;
            if (motor<-255)                //El pulsador 2 decrementa la velocidad de los motores
              motor=0;
            robot.motores(motor,motor);
            robot.lcd.setCursor(0,2);
            robot.lcd.print("Velocidad=");
            robot.lcd.print(motor);
            robot.lcd.print("  ");
          }
        }
        break;
      }
    case 3://comprobar los encoders
      {	
        if (sub_modo == 0)
        {
          robot.lcd.setCursor(0,0);
          robot.lcd.print("ENCODERS");
          robot.encoder1.reset();
          robot.encoder2.reset();           //Al entrar en este menu reseteamos los encoders
          sub_modo= 1;
        }
        else if (sub_modo == 1)
        {				
          if (robot.pulsador(3))
          {
            motor += 25;
            if (motor>255)                //El pulsador 3 incrementa la velocidad de los motores
              motor=0;
            robot.motores(motor,motor);
          }
          if (robot.pulsador(2))
          {
            motor -= 25;
            if (motor<-255)                //El pulsador 2 decrementa la velocidad de los motores
              motor=0;
            robot.motores(motor,motor);
          }
          robot.lcd.setCursor(0,2);
          robot.lcd.print("Cuenta");
          robot.lcd.setCursor(0,3);
          robot.lcd.print(robot.encoder1.read());    //Escribimos los valores en el lcd
          robot.lcd.setCursor(42,3);
          robot.lcd.print(robot.encoder2.read());

          robot.lcd.setCursor(0,4);
          robot.lcd.print("Valor digital");
          robot.lcd.setCursor(0,5);
          robot.lcd.print(digitalRead(ENCODER1));
          robot.lcd.setCursor(42,5);
          robot.lcd.print(digitalRead(ENCODER2));
        }
        break;
      }
    case 4://puerto serie
      {
        if (sub_modo == 0)
        {
          robot.lcd.setCursor(0,0);
          robot.lcd.print("PUERTO SERIE");
          sub_modo = 1;
        }
        else if (sub_modo == 1)
        {
          if(Serial1.available())
          {
            caracter = Serial1.read();	//Cuando recibimos algo en el puerto lo guardamos y escribimos en el lcd
            robot.lcd.setCursor(0,1);
            robot.lcd.print("Recibido: ");
            robot.lcd.write(caracter);
          }
          if (robot.pulsador(2))
            Serial1.write(caracter);  //El pulsador 2 reenvia el ultimo caracter recibido
          if (robot.pulsador(3))
            Serial1.print("Depecabot 3");  //El pulsador 3 enva una cadena
        }		
        break;
      }
    case 5://comprobar la bateria
      {
        if (sub_modo == 0)
        {
          robot.lcd.setCursor(0,0);
          robot.lcd.print("NIVEL BATERIA");

          sub_modo = 1;
        }
        else if (sub_modo == 1)
        {
          bateria=robot.bateria();    //Lectura de la bateria en milivoltios
          robot.lcd.setCursor(0,2);
          robot.lcd.print(bateria);
          robot.lcd.print("V   ");
        }
        break;
      }
    case 6://comprobar minialca_cnys				
      {
        robot.lcd.setCursor(0,0);
        robot.lcd.print("SENSOR LINEA");

        if(robot.sensorLinea.readCalibrated())    //Si la funcion devuelve error lo indicamos en el lcd
        {
          robot.lcd.setCursor(0,2);
          robot.lcd.print("ERROR COM. I2C  ");
        }
        else
        {
          robot.lcd.setCursor(0,1);
          robot.lcd.print("Valores");

          robot.lcd.setCursor(0,2);
          robot.lcd.print(robot.sensorLinea.array[0]);      //Si no devuelve error mostramos los valores
          robot.lcd.print("              ");
          robot.lcd.setCursor(22,2);
          robot.lcd.print(robot.sensorLinea.array[1]);
          robot.lcd.setCursor(44,2);
          robot.lcd.print(robot.sensorLinea.array[2]);
          robot.lcd.setCursor(66,2);
          robot.lcd.print(robot.sensorLinea.array[3]);

          robot.lcd.setCursor(0,3);
          robot.lcd.print("Calibrados");

          robot.lcd.setCursor(0,4);
          robot.lcd.print(robot.sensorLinea.arrayCalibrated[0]);      //Si no devuelve error mostramos los valores
          robot.lcd.print("              ");
          robot.lcd.setCursor(22,4);
          robot.lcd.print(robot.sensorLinea.arrayCalibrated[1]);
          robot.lcd.setCursor(44,4);
          robot.lcd.print(robot.sensorLinea.arrayCalibrated[2]);
          robot.lcd.setCursor(66,4);
          robot.lcd.print(robot.sensorLinea.arrayCalibrated[3]);

          robot.lcd.setCursor(0,5);
          posicion=robot.sensorLinea.posicion();
          robot.lcd.print("Posicion=     ");
          robot.lcd.setCursor(60,5);
          robot.lcd.print(posicion);

        }
        if (robot.pulsador(2))  
        {
          robot.lcd.clear();
          robot.lcd.setCursor(0,0);
          robot.lcd.print("Calibrando");
          robot.lcd.setCursor(0,1);
          robot.lcd.print("Mover sensor");
          robot.lcd.setCursor(0,2);
          robot.lcd.print("durante 5 seg");
          robot.sensorLinea.calibrate();
          robot.lcd.clear();
        }
        break;
      }
    case 7:	//comprobar el servo			
      {
        if(sub_modo==0)
        {
          servo=SERVO_INIT_POS;
          robot.servo1.write(servo);
          robot.lcd.setCursor(0,0);
          robot.lcd.print("SERVO");
          sub_modo=1;
        }
        else
        {
          robot.lcd.setCursor(0,2);
          robot.lcd.print("Posicion=");
          robot.lcd.print(servo);
          robot.lcd.print("  ");

          if (robot.pulsador(2))      //El pulsador 2 gira a la izquierda
          {
            servo += 5;
            if(servo > 180) // Saturación
              servo = 180;
            robot.servo1.write(servo);
          }
          if (robot.pulsador(3))      //El pulsador 3 gira a la derecha
          {
            servo -= 5;
            if(servo < 0) // Saturación
              servo = 0;
            robot.servo1.write(servo);
          }
        }
        break;
      }
    case 8:				
      {
        modo = 1;
        break;
      }
    }	
  }
}

